//
// Created by ze on 2023/11/2.
//

#ifndef ENCODER_UPRE_LOCATION_H
#define ENCODER_UPRE_LOCATION_H
/*********标定全场的步骤**************
*1.先将T1调成0度 ，DETA_T调成90度。
*2.把车拉出去推一圈，随便推。
*3.用上位机程序推出DETA_T的角度。
*4.再沿着某个轴推出一个直线，最后到的点在直线上就行。
*5.用上位机推出的角度算出T1
*6.全场标定完毕，在主机中进行平移中心即可。
**************************************/

/***************************************************************
*		    		需要标定参数，按照标定顺序排列
****************************************************************/
#include "UPRE_GLOBAL_DEFINE.h"

//X轴    脉冲值转换成毫米
#define  PULSE2DIS_ENCODE1 (0.038817f)
//Y轴    脉冲值转换成毫米
#define  PULSE2DIS_ENCODE2 (0.038818f)
////X轴    脉冲值转换成毫米
//#define  PULSE2DIS_ENCODE1 (1.f)
////Y轴    脉冲值转换成毫米
//#define  PULSE2DIS_ENCODE2 (1.f)
//X轴    等比例缩放
#define  ENCODER_DIS1       1.f
//Y轴    等比例缩放
#define  ENCODER_DIS2       1.f


//T2-T1  89.525
#define   DETA_T                    (85.698534959.f)
//码盘1与X轴夹角45.1
#define   Tt1					    (-45.f-1.016908f)
//码盘2与X轴夹角
#define   Tt2 						(45.f-0.561647f)
#define sint1       sinf(location.theta1)
#define sint2       sinf(location.theta2)
#define cost1       cosf(location.theta1)
#define cost2       cosf(location.theta2)
//#define _L1          (location.R1 * location.ALPHA)
//#define _L2          (location.R2 * location.ALPHA)
#define  DIS1       (location.DIS_1 * PULSE2DIS_ENCODE1)
#define  DIS2       (location.DIS_2 * PULSE2DIS_ENCODE2)

typedef struct _Location {
    float DIS_1, DIS_2; //DIS_1:旋转前的x坐标  DIS_2:旋转前的Y坐标
    float ALPHA, gyro_radian, alphaed, Center_X, Center_Y, Whole_X, Whole_Y, Sin_12, Sin_21;
    float R1, R2, theta1, theta2;
    float fail1, fail2;
    float buf_x, buf_y, theta;
    float Last_whole_X, Last_whole_Y; //全场坐标的微分
    float m_spi_Posx, m_spi_Posy; //标定旋转前的全场坐标的微分
    float spi_Posx, spi_Posy; //标定旋转前的全场坐标的积分
    float RECTANG_X, RECTANG_Y;
    float X_coder, Y_coder;
    float k1, k2, k3, k4;
    float m1, m2; //微分
    float dised_1, dised_2;
} Location;

extern Pose Robot_Pose;

void Encoder_On();

void Location_Init(void);

void cal_location(void);

void Clean_Pose(void);

extern Location location;

#endif //ENCODER_UPRE_LOCATION_H
